#include "helper.h"
#include "monitor.h"
#include "reg.h"

extern uint32_t instr;
extern char assembly[80];
extern FILE *trace_fp;
uint32_t mem_read(uint32_t addr, size_t len);
void mem_write(uint32_t addr, size_t len, uint32_t data);

/* decode I-type instrucion with unsigned immediate */
static void decode_imm_type(uint32_t instr)
{

	op_src1->type = OP_TYPE_REG;
	op_src1->reg = (instr & RS_MASK) >> (RT_SIZE + IMM_SIZE);
	op_src1->val = reg_w(op_src1->reg);

	op_src2->type = OP_TYPE_IMM;
	op_src2->imm = instr & IMM_MASK;
	op_src2->val = op_src2->imm;

	op_dest->type = OP_TYPE_REG;
	op_dest->reg = (instr & RT_MASK) >> (IMM_SIZE);
}

make_helper(lui)
{

	decode_imm_type(instr);
	reg_w(op_dest->reg) = (op_src2->val << 16);
	sprintf(assembly, "lui   %s,   0x%04x", REG_NAME(op_dest->reg), op_src2->imm);
    fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (op_src2->val << 16));
}

make_helper(ori)
{

	decode_imm_type(instr);
	reg_w(op_dest->reg) = op_src1->val | op_src2->val;
	sprintf(assembly, "ori   %s,   %s,   0x%04x", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
     fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (op_src1->val | op_src2->val));
}

make_helper(addi)
{
	decode_imm_type(instr);
	int imm = op_src2->val;
	imm = imm << 16;
	imm = imm >> 16;
	uint32_t temp = op_src1->val + imm;
	uint32_t temp1 = (op_src1->val) & 0x80000000;
	uint32_t temp2 = imm & 0x80000000;
	uint32_t temp3 = temp & 0x80000000;
	if ((temp3 != temp1) && (temp3 != temp2))
	{
		// overflow
	}
	else
	{
		reg_w(op_dest->reg) = temp;
	}
	sprintf(assembly, "addi   %s,   %s,   0x%04x", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
	fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (temp));
}

make_helper(addiu)
{
	decode_imm_type(instr);
	int imm = op_src2->val;
	imm = imm << 16;
	imm = imm >> 16;
	int temp_result = op_src1->val + imm;

	// Judge Integer_Overflow
	if (((temp_result & 0x80000000) != (op_src1->val & 0x80000000)) && ((op_src1->val & 0x80000000) == (imm & 0x80000000)))
	{

		// Cause.ExcCode <- 0x0c(Ov)
		cp0_w(R_CAUSE) = (cp0_w(R_CAUSE) & 0xffffff83) | (0x0c << 2);
		// EPC <- pc
		cp0_w(R_EPC) = cpu.pc;
		// Status.EXL <- 1
		cp0_w(R_STATUS) = ((cp0_w(R_STATUS) & 0xfffffffd) | (0x1 << 1));

		// If Ov is in Delay_Slot...
		if (temu_state == JUMP)
		{
			// EPC <- EPC - 4
			cp0_w(R_EPC) -= 4;
			// Cause.BD <- 1
			cp0_w(R_CAUSE) = ((cp0_w(R_CAUSE) & 0x7fffffff) | 0x80000000);
		}

		reg_w(op_dest->reg) = (uint32_t)temp_result;
		sprintf(assembly, "addi    %s, %s, 0x%04x\nWARNING: Inetger_Overflow Exception.", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
		cpu.pc = 0xbfc00380;
	}
	else
	{
		reg_w(op_dest->reg) = (uint32_t)temp_result;
		sprintf(assembly, "addi    %s, %s, 0x%04x", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
	}
	  fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (temp_result));
}

make_helper(slti)
{
	decode_imm_type(instr);
	int imm = op_src2->val;
	imm = imm << 16;
	imm = imm >> 16;
	int temp = op_src1->val;
	int  res= temp < imm ? 1 : 0;
	reg_w(op_dest->reg) =res;
	sprintf(assembly, "slti   %s,   %s,   0x%04x", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
	fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (res));
}

make_helper(sltiu)
{
	decode_imm_type(instr);
	int imm = op_src2->val;
	imm = imm << 16;
	imm = imm >> 16;
	int res = op_src1->val < (uint32_t)imm ? 1 : 0;
	reg_w(op_dest->reg) =res;
	sprintf(assembly, "slti   %s,   %s,   0x%04x", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
    fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (res));
}

make_helper(andi)
{
	decode_imm_type(instr);
	uint32_t imm = (uint32_t)op_src2->val;
	reg_w(op_dest->reg) = op_src1->val & imm;
	sprintf(assembly, "andi   %s,   %s,   0x%04x", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
    fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, ( op_src1->val & imm));
}

make_helper(xori)
{
	decode_imm_type(instr);
	uint32_t imm = (uint32_t)op_src2->val;
	reg_w(op_dest->reg) = op_src1->val ^ imm;
	sprintf(assembly, "xori   %s,   %s,   0x%04x", REG_NAME(op_dest->reg), REG_NAME(op_src1->reg), op_src2->imm);
     fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (op_src1->val ^ imm));
}


make_helper(beq)
{
	decode_imm_type(instr);
	uint32_t offset = ((int)(op_src2->val << (32 - IMM_SIZE))) >> (32 - IMM_SIZE - 2);
	int target_offset = op_src1->val == reg_w(op_dest->reg) ? offset : 0;
	cpu.pc += target_offset;
	sprintf(assembly, "beq   %s,   %s,   0x%04x", REG_NAME(op_src1->reg), REG_NAME(op_dest->reg), op_src2->imm);
	temu_state = JUMP;
}

make_helper(bne)
{
	decode_imm_type(instr);
	uint32_t offset = ((int)(op_src2->val << (32 - IMM_SIZE))) >> (32 - IMM_SIZE - 2);
	int target_offset = op_src1->val != reg_w(op_dest->reg) ? offset : 0;
	cpu.pc += target_offset;
	sprintf(assembly, "bne   %s,   %s,   0x%04x", REG_NAME(op_src1->reg), REG_NAME(op_dest->reg), op_src2->imm);
	temu_state = JUMP;
}

make_helper(bgtz)
{
	decode_imm_type(instr);
	uint32_t offset = ((int)(op_src2->val << (32 - IMM_SIZE))) >> (32 - IMM_SIZE - 2);
	int target_offset = op_src1->val > 0 ? offset : 0;
	cpu.pc += target_offset;
	sprintf(assembly, "bgtz   %s,   0x%04x", REG_NAME(op_src1->reg), op_src2->imm);
	temu_state = JUMP;
}

make_helper(blez)
{
	decode_imm_type(instr);
	uint32_t offset = ((int)(op_src2->val << (32 - IMM_SIZE))) >> (32 - IMM_SIZE - 2);
	int target_offset = op_src1->val <= 0 ? offset : 0;
	cpu.pc += target_offset;
	sprintf(assembly, "blez   %s,   0x%04x", REG_NAME(op_src1->reg), op_src2->imm);
	temu_state = JUMP;
}

make_helper(b)
{
	decode_imm_type(instr);
	uint32_t offset = ((int)(op_src2->val << (32 - IMM_SIZE))) >> (32 - IMM_SIZE - 2);
	int target_offset = 0;
	switch (op_dest->reg)
	{
	case 0x1: // bgez
		target_offset = (int)op_src1->val >= 0 ? offset : 0;
		cpu.pc += target_offset;
		sprintf(assembly, "bgez   %s,   0x%04x", REG_NAME(op_src1->reg), op_src2->imm);
		break;
	case 0x0: // bltz
		target_offset = (int)op_src1->val < 0 ? offset : 0;
		cpu.pc += target_offset;
		sprintf(assembly, "bltz   %s,   0x%04x", REG_NAME(op_src1->reg), op_src2->imm);
		break;
	case 0x11: // bgezal
		cpu.gpr[31]._32 = cpu.pc + 8;
		target_offset = (int)op_src1->val >= 0 ? offset : 0;
		cpu.pc += target_offset;
		sprintf(assembly, "bgezal   %s,   0x%04x", REG_NAME(op_src1->reg), op_src2->imm);
		break;
	case 0x10: // bltzal
		cpu.gpr[31]._32 = cpu.pc + 8;
		target_offset = (int)op_src1->val < 0 ? offset : 0;
		cpu.pc += target_offset;
		sprintf(assembly, "bltzal   %s,   0x%04x", REG_NAME(op_src1->reg), op_src2->imm);
		break;
	default:
		cpu.pc += target_offset;
		break;
	}
	temu_state = JUMP;
}

make_helper(lb)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	int mem_val = (((int)mem_read(address, 1) << 24) >> 24);
	reg_w(op_dest->reg)  = mem_val;
	sprintf(assembly, "lb   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
    fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (mem_val));
}

make_helper(lbu)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	 uint32_t mem_val= ((mem_read(address, 1) << 24) >> 24);
	 reg_w(op_dest->reg) = mem_val;
	sprintf(assembly, "lbu   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
     fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (mem_val));
}

make_helper(lh)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	// Judge Address_Load Error
	if ((address & 1) != 0)
	{
		// Cause.ExcCode <- 0x04(AdEL)
		cp0_w(R_CAUSE) = (cp0_w(R_CAUSE) & 0xffffff83) | (0x04 << 2);
		// BadVAddr <- address
		cp0_w(R_BADVADDR) = address;
		// EPC <- pc
		cp0_w(R_EPC) = cpu.pc;
		// Status.EXL <- 1
		cp0_w(R_STATUS) = ((cp0_w(R_STATUS) & 0xfffffffd) | (0x1 << 1));

		// If AdEL is in Delay_Slot...
		if (temu_state == JUMP)
		{
			// EPC <- EPC - 4
			cp0_w(R_EPC) -= 4;
			// Cause.BD <- 1
			cp0_w(R_CAUSE) = ((cp0_w(R_CAUSE) & 0x7fffffff) | 0x80000000);
		}

		panic("Address is not a multiple of 2\n");
		sprintf(assembly, "WARNING: Address_Load Exception.\n");
		cpu.pc = 0xbfc00380;
	}
	else
	{
		int mem_val = (((int)mem_read(address, 2) << 16) >> 16);
		 reg_w(op_dest->reg)=mem_val;
		sprintf(assembly, "lh   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
	     fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (mem_val));
	}
}

make_helper(lhu)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	if ((address & 1) != 0)
	{
		// Cause.ExcCode <- 0x04(AdEL)
		cp0_w(R_CAUSE) = (cp0_w(R_CAUSE) & 0xffffff83) | (0x04 << 2);
		// BadVAddr <- address
		cp0_w(R_BADVADDR) = address;
		// EPC <- pc
		cp0_w(R_EPC) = cpu.pc;
		// Status.EXL <- 1
		cp0_w(R_STATUS) = ((cp0_w(R_STATUS) & 0xfffffffd) | (0x1 << 1));

		// If AdEL is in Delay_Slot...
		if (temu_state == JUMP)
		{
			// EPC <- EPC - 4
			cp0_w(R_EPC) -= 4;
			// Cause.BD <- 1
			cp0_w(R_CAUSE) = ((cp0_w(R_CAUSE) & 0x7fffffff) | 0x80000000);
		}

		panic("Address is not a multiple of 2\n");
		sprintf(assembly, "WARNING: Address_Load Exception.\n");
		cpu.pc = 0xbfc00380;
	}
	else
	{
		uint32_t mem_val=((mem_read(address, 2) << 16) >> 16);
		reg_w(op_dest->reg) = mem_val;
		sprintf(assembly, "lhu   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
	     fprintf(trace_fp, "0x%04x     %02d     0x%04x", cpu.pc, op_dest->reg, (mem_val));
	}
}

make_helper(lw)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	if ((address & 0x3) != 0)
	{
		// Cause.ExcCode <- 0x04(AdEL)
		cp0_w(R_CAUSE) = (cp0_w(R_CAUSE) & 0xffffff83) | (0x04 << 2);
		// BadVAddr <- address
		cp0_w(R_BADVADDR) = address;
		// EPC <- pc
		cp0_w(R_EPC) = cpu.pc;
		// Status.EXL <- 1
		cp0_w(R_STATUS) = ((cp0_w(R_STATUS) & 0xfffffffd) | (0x1 << 1));

		// If AdEL is in Delay_Slot...
		if (temu_state == JUMP)
		{
			// EPC <- EPC - 4
			cp0_w(R_EPC) -= 4;
			// Cause.BD <- 1
			cp0_w(R_CAUSE) = ((cp0_w(R_CAUSE) & 0x7fffffff) | 0x80000000);
		}

		panic("Address is not a multiple of 4\n");
		sprintf(assembly, "WARNING: Address_Load Exception.\n");
		cpu.pc = 0xbfc00380;
	}
	else
	{   uint32_t mem_val= mem_read(address, 4);
		reg_w(op_dest->reg) = mem_val;
		sprintf(assembly, "lw   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
	     fprintf(trace_fp, "0x%04x     %02d     0x%04x\n", cpu.pc, op_dest->reg, (mem_val));
	}
}

make_helper(sb)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	uint32_t byte = reg_w(op_dest->reg) & 0xff;
	mem_write(address, 1, byte);
	sprintf(assembly, "sb   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
    
}

make_helper(sh)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	uint32_t hw = reg_w(op_dest->reg) & 0xffff;
	if ((address & 0x1) != 0)
	{
		// Cause.ExcCode <- 0x05(AdES)
		cp0_w(R_CAUSE) = (cp0_w(R_CAUSE) & 0xffffff83) | (0x05 << 2);
		// BadVAddr <- address
		cp0_w(R_BADVADDR) = address;
		// EPC <- pc
		cp0_w(R_EPC) = cpu.pc;
		// Status.EXL <- 1
		cp0_w(R_STATUS) = ((cp0_w(R_STATUS) & 0xfffffffd) | (0x1 << 1));

		// If AdES is in Delay_Slot...
		if (temu_state == JUMP)
		{
			// EPC <- EPC - 4
			cp0_w(R_EPC) -= 4;
			// Cause.BD <- 1
			cp0_w(R_CAUSE) = ((cp0_w(R_CAUSE) & 0x7fffffff) | 0x80000000);
		}

		panic("Address is not a multiple of 2\n");
		sprintf(assembly, "WARNING: Address_Store Exception.\n");
		cpu.pc = 0xbfc00380;
	}
	else
	{
		mem_write(address, 2, hw);
	}
	sprintf(assembly, "sh   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
}

make_helper(sw)
{
	decode_imm_type(instr);
	uint32_t address = op_src1->val + (((int)op_src2->val << 16) >> 16);
	address = address & 0x1fffffff;
	uint32_t w = reg_w(op_dest->reg);
	if ((address & 0x3) != 0)
	{
		// Cause.ExcCode <- 0x05(AdES)
		cp0_w(R_CAUSE) = (cp0_w(R_CAUSE) & 0xffffff83) | (0x05 << 2);
		// BadVAddr <- address
		cp0_w(R_BADVADDR) = address;
		// EPC <- pc
		cp0_w(R_EPC) = cpu.pc;
		// Status.EXL <- 1
		cp0_w(R_STATUS) = ((cp0_w(R_STATUS) & 0xfffffffd) | (0x1 << 1));

		// If AdES is in Delay_Slot...
		if (temu_state == JUMP)
		{
			// EPC <- EPC - 4
			cp0_w(R_EPC) -= 4;
			// Cause.BD <- 1
			cp0_w(R_CAUSE) = ((cp0_w(R_CAUSE) & 0x7fffffff) | 0x80000000);
		}

		panic("Address is not a multiple of 4\n");
		sprintf(assembly, "WARNING: Address_Store Exception.\n");
		cpu.pc = 0xbfc00380;
	}
	else
	{
		mem_write(address, 4, w);
		sprintf(assembly, "sw   %s   0x%04x(%s)", REG_NAME(op_dest->reg), op_src2->imm, REG_NAME(op_src1->reg));
	}
}